#include <fstream>
#include <iostream>

using namespace std;

struct F3d
{
    double x, y, z;
};

typedef struct F3dList
{
	F3d pointData;
	F3dList* pNext;
} F3dNode;

//Compare Method to judge whether test point is in the same side with the triangle base point
//pTrPoint1,2,3: 3 Triangle Points
//pTestPoint: Test Point on the Ray
//pRayBase: The Base Point of the Ray
bool fnCompareRayPosition(const F3d* pTrPoint1,const F3d* pTestPoint, 
						  const F3d* pRayBase, const F3d* pTrPoint2,const F3d* pTrPoint3)
{
	//Get Plane Direction
	F3d oPlaneDirection;
	oPlaneDirection.x = (pTrPoint2->y - pRayBase->y)*(pTrPoint3->z - pRayBase->z)
					  - (pTrPoint2->z - pRayBase->z)*(pTrPoint3->y - pRayBase->y);

	oPlaneDirection.y = (pTrPoint2->z - pRayBase->z)*(pTrPoint3->x - pRayBase->x)
					  - (pTrPoint2->x - pRayBase->x)*(pTrPoint3->z - pRayBase->z);

	oPlaneDirection.z = (pTrPoint2->x - pRayBase->x)*(pTrPoint3->y - pRayBase->y)
					  - (pTrPoint2->y - pRayBase->y)*(pTrPoint3->x - pRayBase->x);
	
	//Detect the test triangle point side
	double dTest1 = (pTrPoint1->x - pRayBase->x)*oPlaneDirection.x
				  + (pTrPoint1->y - pRayBase->y)*oPlaneDirection.y
				  + (pTrPoint1->z - pRayBase->z)*oPlaneDirection.z;

	if (dTest1 == 0)
		return false;   //If ray base point is in the same plane of triangle, return false
	else
	{
		double dTest2 = (pTestPoint->x - pRayBase->x)*oPlaneDirection.x
					  + (pTestPoint->y - pRayBase->y)*oPlaneDirection.y
					  + (pTestPoint->z - pRayBase->z)*oPlaneDirection.z;

		//Judge whether these two points are in the same side
		if (dTest2/dTest1 >= 0)
			return true;
		else
			return false;
	}
};

//Compute Unit for one ray and triangle
//For ray, you need to input start point and direciton vector
//For Triangle, you need to input array F3d[3] to show 3 points.
//Result will be written in pIntersectionList
bool fnDetectIntersection(const F3d* pRayOrigins,const F3d* pRayDirection,const F3d* pTrianglePoint1)
{
	//return true;
	bool bResult = false;
	F3d* pTest = new F3d();
	pTest->x = pRayOrigins->x + pRayDirection->x;
	pTest->y = pRayOrigins->y + pRayDirection->y;
	pTest->z = pRayOrigins->z + pRayDirection->z;

	if(!fnCompareRayPosition(&pTrianglePoint1[0],pTest,pRayOrigins,&pTrianglePoint1[1],&pTrianglePoint1[2]))
		bResult = false;
	else if(!fnCompareRayPosition(&pTrianglePoint1[1],pTest,pRayOrigins,&pTrianglePoint1[0],&pTrianglePoint1[2]))
		bResult = false;
	else if(!fnCompareRayPosition(&pTrianglePoint1[2],pTest,pRayOrigins,&pTrianglePoint1[0],&pTrianglePoint1[1]))
		bResult = false;
	else
		bResult = true;
	delete pTest;
	return bResult;
};

//Calculate the intersection point after judgement, this function will not include intersection detect
//pRayBase, pRayDirection: The base point and direction of the ray
//pTrPoint: The first point of the triangle in the point list
//ppCurrentNode: End of result node link list 
void fnCalculateIntersection(const F3d* pRayBase, const F3d* pRayDirection, const F3d* pTrPoint, F3dNode** ppCurrentNode)
{
	//Get Plane Direction
	F3d oPlaneDirection;
	oPlaneDirection.x = (pTrPoint[1].y - pTrPoint[0].y)*(pTrPoint[2].z - pTrPoint[0].z)
					  - (pTrPoint[1].z - pTrPoint[0].z)*(pTrPoint[2].y - pTrPoint[0].y);

	oPlaneDirection.y = (pTrPoint[1].z - pTrPoint[0].z)*(pTrPoint[2].x - pTrPoint[0].x)
					  - (pTrPoint[1].x - pTrPoint[0].x)*(pTrPoint[2].z - pTrPoint[0].z);

	oPlaneDirection.z = (pTrPoint[1].x - pTrPoint[0].x)*(pTrPoint[2].y - pTrPoint[0].y)
					  - (pTrPoint[1].y - pTrPoint[0].y)*(pTrPoint[2].x - pTrPoint[0].x);

	//Calculate InterSection Factor
	double dFactor = (oPlaneDirection.x*(pTrPoint->x - pRayBase->x) + oPlaneDirection.y*(pTrPoint->y - pRayBase->y) + oPlaneDirection.z*(pTrPoint->z - pRayBase->z))
				   / (pRayDirection->x*oPlaneDirection.x + pRayDirection->y*oPlaneDirection.y + pRayDirection->z*oPlaneDirection.z);
	
	//Calculate InterSection Point
	F3dNode* pIntersection = new F3dNode();
	pIntersection->pointData.x = pRayBase->x + pRayDirection->x * dFactor;
	pIntersection->pointData.y = pRayBase->y + pRayDirection->y * dFactor;
	pIntersection->pointData.z = pRayBase->z + pRayDirection->z * dFactor;
	//Add Intersection node into node list
	pIntersection->pNext = NULL;
	(*ppCurrentNode)->pNext = pIntersection;
	*ppCurrentNode = (*ppCurrentNode)->pNext;
};

//Main Compute Process
void fnComputeProcess(const int iNumOfTriangles,const F3d* pVertexBuffer, 
					  const int iNumOfRays, const F3d* pRayOrigins,F3d* pRayDirections,
					  int* const pNumOfIntersections, F3dList* const pIntersectionList)
{
	F3dNode* pNode = pIntersectionList;
	for(int i = 0; i< iNumOfRays; i++)
	{
		for(int j = 0; j<iNumOfTriangles; j++)
		{
			if(fnDetectIntersection(&pRayOrigins[i],&pRayDirections[i],&pVertexBuffer[3*j]))
			{
				//Calculate InterSection
				(*pNumOfIntersections)++;
				fnCalculateIntersection(&pRayOrigins[i],&pRayDirections[i],&pVertexBuffer[3*j],&pNode);
			}
		}
	}

};